#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

class BaseController {
public:
    BaseController() : x_(0.0), y_(0.0), theta_(0.0) {
        // 初始化ROS节点
        ros::NodeHandle nh;

        // 获取参数
        nh.param("wheel_radius", wheel_radius_, 0.1);
        nh.param("wheel_base", wheel_base_, 0.5);

        // 订阅速度命令
        cmd_vel_sub_ = nh.subscribe("/cmd_vel", 10, &BaseController::cmdVelCallback, this);

        // 发布里程计数据
        odom_pub_ = nh.advertise<nav_msgs::Odometry>("/odom", 50);

        // TF广播器
        tf_broadcaster_ = std::make_shared<tf::TransformBroadcaster>();

        // 初始化时间
        last_time_ = ros::Time::now();
    }

private:
    double wheel_radius_;
    double wheel_base_;
    double x_, y_, theta_;
    ros::Time last_time_;
    ros::Subscriber cmd_vel_sub_;
    ros::Publisher odom_pub_;
    std::shared_ptr<tf::TransformBroadcaster> tf_broadcaster_;

    void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        // 获取当前时间
        ros::Time current_time = ros::Time::now();
        double dt = (current_time - last_time_).toSec();

        // 解析速度命令
        double linear_x = msg->linear.x;
        double angular_z = msg->angular.z;

        // 计算新的位置和姿态
        x_ += linear_x * std::cos(theta_) * dt;
        y_ += linear_x * std::sin(theta_) * dt;
        theta_ += angular_z * dt;

        // 发布里程计数据
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_footprint";

        odom.pose.pose.position.x = x_;
        odom.pose.pose.position.y = y_;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta_);

        odom.twist.twist.linear.x = linear_x;
        odom.twist.twist.angular.z = angular_z;

        odom_pub_.publish(odom);

        // 发布TF变换
        geometry_msgs::TransformStamped odom_tf;
        odom_tf.header.stamp = current_time;
        odom_tf.header.frame_id = "odom";
        odom_tf.child_frame_id = "base_footprint";

        odom_tf.transform.translation.x = x_;
        odom_tf.transform.translation.y = y_;
        odom_tf.transform.translation.z = 0.0;
        odom_tf.transform.rotation = odom.pose.pose.orientation;

        tf_broadcaster_->sendTransform(odom_tf);

        // 更新时间
        last_time_ = current_time;
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "base_controller");
    BaseController base_controller;
    ros::spin();
    return 0;
}
